from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import rclpy


def main():
    # 初始化ROS2客户端
    rclpy.init()
    # 创建一个BasicNavigator对象
    navigator = BasicNavigator()
    # 创建一个PoseStamped对象
    initial_pose = PoseStamped()
    # 设置初始位姿的参考坐标系
    initial_pose.header.frame_id = 'map'
    # 设置初始位姿的时间戳
    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
    # 设置初始位姿的位置
    initial_pose.pose.position.x = 0.0
    initial_pose.pose.position.y = 0.0
    # 设置初始位姿的朝向
    initial_pose.pose.orientation.w = 1.0
    # 设置初始位姿
    navigator.setInitialPose(initial_pose)
    # 等待导航2激活
    navigator.waitUntilNav2Active()
    # 进入ROS2事件循环
    rclpy.spin(navigator)
    # 关闭ROS2客户端
    rclpy.shutdown()

if __name__ == '__main__':
    main()